#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include<tf/transform_datatypes.h>

bool is_get_tracker_first_gps_odom_ = false;
tf::StampedTransform tf_tracker_first_,tf_tracker_last_;
tf::Vector3 tracker_position_last_;

ros::Publisher tracker_odom_pub_,tracker_odom_path_pub_, tracker_velocity_pub_;
nav_msgs::Path tracker_odom_path_;

bool is_get_target_first_gps_odom_ = false;
tf::StampedTransform tf_target_last_,tf_target_first_;
tf::Vector3 target_position_last_;

ros::Publisher target_odom_pub_,target_odom_path_pub_;
nav_msgs::Path target_odom_path_;

void tracker_gps_odom_CB(const nav_msgs::OdometryConstPtr& tracker_gps_odom){
    if(!is_get_tracker_first_gps_odom_){
        tf::Transform tf_temp;
        tf::poseMsgToTF(tracker_gps_odom->pose.pose, tf_temp);
        tf_tracker_first_ = tf::StampedTransform(tf_temp, tracker_gps_odom->header.stamp, "gps", "tracker");
        tf_tracker_last_ = tf_tracker_first_;
        is_get_tracker_first_gps_odom_ = true;
        ROS_INFO_STREAM("get first tracker gps odom: "<<tf_tracker_first_.stamp_.toSec()<<" "<<
            tf_tracker_first_.getOrigin().getX()<<" "<<tf_tracker_first_.getOrigin().getY()<<" "<<tf_tracker_first_.getOrigin().getZ());
        return;
    }
    tf::Transform tf_temp;
    tf::poseMsgToTF(tracker_gps_odom->pose.pose, tf_temp);
    tf::StampedTransform tf_tracker_cur(tf_temp, tracker_gps_odom->header.stamp, "gps", "tracker");
    tf::Transform tf_delta = tf_tracker_first_.inverse()*tf_tracker_cur;
    double roll_temp, yaw_temp,pitch_temp;
    tf::Matrix3x3(tf_delta.getRotation()).getRPY(roll_temp, pitch_temp, yaw_temp);
    // ROS_DEBUG_STREAM("r p y: "<<roll_temp<<" "<<pitch_temp<<" "<<yaw_temp);
    tf_delta.setRotation(tf::Quaternion(0, 0,yaw_temp));
    tf_delta.setOrigin(tf::Vector3(tf_delta.getOrigin().getX(), -tf_delta.getOrigin().getY(), 0));
    tf::Vector3 tracker_position_cur(tf_delta.getOrigin().getX(),tf_delta.getOrigin().getY(),tf_delta.getOrigin().getZ());
    tf::Vector3 velocity_world = (tracker_position_cur-tracker_position_last_)/(tf_tracker_cur.stamp_-tf_tracker_last_.stamp_).toSec();
    // ROS_DEBUG_STREAM("velocity: "<<velocity_world[0]<<" "<<velocity_world[1]<<" "<<velocity_world[2]);
    tf_tracker_last_ = tf_tracker_cur;
    tracker_position_last_ = tracker_position_cur;

    //broadcast tf
    static tf::TransformBroadcaster broadcaster;//1.定义一个广播broadcaster
    broadcaster.sendTransform(tf::StampedTransform(tf_delta, ros::Time::now(), "world", "tracker"));

    //pub odom
    nav_msgs::Odometry tracker_odom_cur_msg;
    tracker_odom_cur_msg.header.frame_id = "world";
    tracker_odom_cur_msg.child_frame_id = "tracker";
    tracker_odom_cur_msg.header.stamp = ros::Time::now();
    geometry_msgs::Pose pose_msg_temp;
    tf::poseTFToMsg(tf_delta, pose_msg_temp);
    tracker_odom_cur_msg.pose.pose = pose_msg_temp;
    tracker_odom_cur_msg.twist.twist.linear.x = velocity_world[0];
    tracker_odom_cur_msg.twist.twist.linear.y = velocity_world[1];
    tracker_odom_cur_msg.twist.twist.linear.z = velocity_world[2];
    tracker_odom_pub_.publish(tracker_odom_cur_msg);

    //pub path
    geometry_msgs::PoseStamped pose_stamped_temp;           
    pose_stamped_temp.pose=pose_msg_temp;
    pose_stamped_temp.header.stamp=ros::Time::now();
    pose_stamped_temp.header.frame_id="world";
    tracker_odom_path_.header=pose_stamped_temp.header;
    tracker_odom_path_.poses.emplace_back(pose_stamped_temp);
    tracker_odom_path_pub_.publish(tracker_odom_path_);

    //pub velocity
    nav_msgs::Path velocity_show;
    pose_stamped_temp.header.stamp = ros::Time::now() - ros::Duration(0.02);
    velocity_show.poses.emplace_back(pose_stamped_temp);
    pose_stamped_temp.header.stamp = ros::Time::now();
    pose_stamped_temp.pose.position.x = tracker_odom_cur_msg.twist.twist.linear.x;
    pose_stamped_temp.pose.position.y = tracker_odom_cur_msg.twist.twist.linear.y;
    pose_stamped_temp.pose.position.z = tracker_odom_cur_msg.twist.twist.linear.z;
    velocity_show.poses.emplace_back(pose_stamped_temp);
    velocity_show.header = pose_stamped_temp.header;
    tracker_velocity_pub_.publish(velocity_show);
}

void target_gps_odom_CB(const nav_msgs::OdometryConstPtr& target_gps_odom){
    if(!is_get_tracker_first_gps_odom_){
        return;
    }
    if(!is_get_target_first_gps_odom_){
        tf::Transform tf_temp;
        tf::poseMsgToTF(target_gps_odom->pose.pose, tf_temp);
        // tf_temp = tf_target_convert_*tf_temp;
        tf_target_last_ = tf::StampedTransform(tf_temp, target_gps_odom->header.stamp, "gps", "target");
        tf_target_first_ = tf_target_last_;
        is_get_target_first_gps_odom_ = true;
        return;
    }
    //cal target first with tracker first angle
    double target_roll_first, target_yaw_first,target_pitch_first;
    tf::Matrix3x3(tf_target_first_.getRotation()).getRPY(target_roll_first, target_pitch_first, target_yaw_first);
    ROS_DEBUG_STREAM("target first r p y: "<<target_roll_first<<" "<<target_pitch_first<<" "<<target_yaw_first);
    double tracker_roll_first, tracker_yaw_first,tracker_pitch_first;
    tf::Matrix3x3(tf_tracker_first_.getRotation()).getRPY(tracker_roll_first, tracker_pitch_first, tracker_yaw_first);
    ROS_DEBUG_STREAM("tracker first r p y: "<<tracker_roll_first<<" "<<tracker_pitch_first<<" "<<tracker_yaw_first);

    tf::Transform tf_temp;
    tf::poseMsgToTF(target_gps_odom->pose.pose, tf_temp);
    // tf_temp = tf_target_convert_*tf_temp;
    tf::StampedTransform tf_target_cur(tf_temp, target_gps_odom->header.stamp, "gps", "target");
    tf::Transform tf_delta = tf_target_first_.inverse()*tf_target_cur;
    tf::Vector3 delta_position(tf_delta.getOrigin().getX() -tf_target_first_.getOrigin().getY() + tf_tracker_first_.getOrigin().getX(),
                                               tf_delta.getOrigin().getY() + tf_target_first_.getOrigin().getX() - tf_tracker_first_.getOrigin().getY(),0);

    tf::Transform tf_delta_convert;
    tf_delta_convert.setOrigin(tf::Vector3(0,0,0));
    tf_delta_convert.setRotation(tf::Quaternion(0,0, -target_yaw_first+tracker_yaw_first));
    delta_position = tf_delta_convert*delta_position;
    tf_delta.setOrigin(delta_position);

    double roll_temp, yaw_temp,pitch_temp;
    tf::Matrix3x3(tf_delta.getRotation()).getRPY(roll_temp, pitch_temp, yaw_temp);
    ROS_DEBUG_STREAM("target r p y: "<<roll_temp<<" "<<pitch_temp<<" "<<yaw_temp);
    tf_delta.setRotation(tf::Quaternion(0, 0,yaw_temp));
    tf::Vector3 target_position_cur(tf_delta.getOrigin().getX(),tf_delta.getOrigin().getY(),tf_delta.getOrigin().getZ());
    ROS_DEBUG_STREAM("target position: "<<target_position_cur[0]<<" "<<target_position_cur[1]<<" "<<target_position_cur[2]);
    tf::Vector3 velocity_world = (target_position_cur-target_position_last_)/(tf_target_cur.stamp_-tf_target_last_.stamp_).toSec();
    ROS_DEBUG_STREAM("target velocity: "<<velocity_world[0]<<" "<<velocity_world[1]<<" "<<velocity_world[2]);
    tf_target_last_ = tf_target_cur;
    target_position_last_ = target_position_cur;

    //pub odom
    nav_msgs::Odometry target_odom_cur_msg;
    target_odom_cur_msg.header.frame_id = "world";
    target_odom_cur_msg.child_frame_id = "target";
    target_odom_cur_msg.header.stamp = ros::Time::now();
    geometry_msgs::Pose pose_msg_temp;
    tf::poseTFToMsg(tf_delta, pose_msg_temp);
    target_odom_cur_msg.pose.pose = pose_msg_temp;
    target_odom_cur_msg.twist.twist.linear.x = velocity_world[0];
    target_odom_cur_msg.twist.twist.linear.y = velocity_world[1];
    target_odom_cur_msg.twist.twist.linear.z = velocity_world[2];
    target_odom_pub_.publish(target_odom_cur_msg);

    //pub path
    geometry_msgs::PoseStamped pose_stamped_temp;           
    pose_stamped_temp.pose=pose_msg_temp;
    pose_stamped_temp.header.stamp=ros::Time::now();
    pose_stamped_temp.header.frame_id="world";
    target_odom_path_.header=pose_stamped_temp.header;
    target_odom_path_.poses.emplace_back(pose_stamped_temp);
    target_odom_path_pub_.publish(target_odom_path_);

}

int main(int argc, char **argv){
    if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug))
    {
        ros::console::notifyLoggerLevelsChanged();
    }

    ros::init(argc, argv, "handle_gps");
    ros::NodeHandle nh;

    ros::Subscriber tracker_gps_odom_sub = nh.subscribe("/novatel/oem7/odom", 10, tracker_gps_odom_CB); 
    ros::Subscriber target_gps_odom_sub = nh.subscribe("/target/novatel/oem7/odom", 10, target_gps_odom_CB); 

    tracker_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/tracker/odom", 1);
    tracker_odom_path_pub_ = nh.advertise<nav_msgs::Path>("/tracker/odom/path", 10,true);
    tracker_velocity_pub_ = nh.advertise<nav_msgs::Path>("/tracker/odom/velocity", 10, true);

    target_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/target/odom", 1);
    target_odom_path_pub_ = nh.advertise<nav_msgs::Path>("/target/odom/path", 10,true);

    ros::spin();

    return 0;
}